#include "SerialConnection.h"
#include <fcntl.h>
#include <signal.h>
#include <termios.h>
#include <pthread.h>
#include <unistd.h>


void* inputDriver(void* data) {
	SerialConnection* conn = (SerialConnection*) data;
	char buf[256];
	int i, j;
	while (conn->run) {
		pthread_mutex_lock(conn->mutex);
		i = read(conn->fd, buf, sizeof(buf));
		if (i < 0)
			;
		// printf("Error at modem input: %d\n", i);
		else if (i > 0) {
			for (j = 0; j < i; j++)
				if(conn->inBuffer.freeSpace()) {
				conn->inBuffer.write(buf[j]);
			}
		}
		pthread_mutex_unlock(conn->mutex);
		usleep((int) (100.0));
	}
	return NULL;
}
/*
 * Forward serial input from modem to serialX buffer. Simulate what Ardupilot interrupt does.
 */
void* outputDriver(void* data) {
	SerialConnection* conn = (SerialConnection*) data;
	char buf[1];
	while (conn->run) {
		pthread_mutex_lock(conn->mutex);
		if (conn->outBuffer.available()) {
			buf[0] = conn->outBuffer.read();
			//printf("%c", buf[0]);
			int i = write(conn->fd, buf, 1);
			if (i < 0)
				printf("Error at modem output: %d\n", i);
		}
		pthread_mutex_unlock(conn->mutex);
		usleep((int) (100.0));
	}
	return NULL;
}

SerialConnection::SerialConnection(pthread_mutex_t* mutex) :
		mutex(mutex),
		run(true),
		inBuffer(256),
		outBuffer(256) {
}

void SerialConnection::init(char* name, speed_t baudRate) {

	fd = open(name, O_RDWR | O_NOCTTY | O_NDELAY);

	fcntl(fd, F_SETFL, FNDELAY);

	struct termios options;
	/* get the current options */
	tcgetattr(fd, &options);

	/* set raw input, 1 second timeout */
	options.c_cflag |= (CLOCAL | CREAD);
	options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
	options.c_oflag &= ~OPOST;
	options.c_cc[VMIN] = 0;
	options.c_cc[VTIME] = 10;

	cfsetspeed(&options, baudRate);

	/* set the options */
	tcsetattr(fd, TCSANOW, &options);
}

void SerialConnection::start() {
	pthread_create(&inputThread, NULL, inputDriver, this);
	pthread_create(&outputThread, NULL, outputDriver, this);
}

void SerialConnection::stop() {
	run = false;
	usleep(1000000);
	close(fd);
}

size_t SerialConnection::available() {
	return inBuffer.available();
}

char SerialConnection::read() {
	return inBuffer.read();
}

size_t SerialConnection::freeSpace() {
	return outBuffer.freeSpace();
}

/*
void SerialConnection::write(char data) {
	outBuffer.write(data);
}
*/

bool SerialConnection::tryWrite(char data) {
	outBuffer.write(data);
	return true;
}

void SerialConnection::clear() {
	inBuffer.clear();
}
